#ifndef LASER_OBSTA_AVOID_HPP
#define LASER_OBSTA_AVOID_HPP

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <agv_msgs/msg/avoid_obsta_info.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <std_msgs/msg/string.hpp>
// #include <std_msgs/msg/time.hpp>
#include <builtin_interfaces/msg/time.hpp>

#include <scenes_judge.hpp>
#include <tinyxml2.h>
#include <unordered_map>
#include <vector>
#include <mutex>
#include <string>
#include <memory>

#define EVENT_STRING "9527"
#define EVENT_INT 9527

using namespace std;
using namespace tinyxml2;

namespace AgvObstaAvoid {

class LaserObstaAvoid 
{
private:
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr path_sub_;
    rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odom_sub_;

    bool last_stop_;
    bool vehicle_forward_;
    double cur_speed_[3];
    
    bool scan_check_size_;
    int num_frame_;
    
    struct uCheck {
        int stop_num = 0;
        int slow_num = 0;
        int warn_num = 0;
    };

    struct endPath {
        bool flag = false;
        int nodeId = 0;
        double remain_distance = 0.0;
    };
    
public:
    builtin_interfaces::msg::Time lidar_time_1_;
    builtin_interfaces::msg::Time lidar_time_2_;

    uCheck lidar_1_;
    uCheck lidar_2_;

    endPath end_path_;

    struct uPoint {
        double x;
        double y;
    };

    struct uRegion {
        string name;
        std::vector<uPoint> poinlist;
    };

    struct uArea {
        int name;
        std::vector<uRegion> regionlist;
    };

    struct uEquipment {
        int name;
        int pose;
        string topicname;
        string inversion;
        unordered_map<int, uArea> arealist;
    };

    vector<uEquipment> laser_equip_list_;
    unordered_map<int, vector<uPoint>> laser_point_;
    std::mutex scan_mutex_;

    LaserObstaAvoid();
    ~LaserObstaAvoid() = default;
    
    bool LaserReadXml(XMLElement* element);
    void ShowEquipParam(uEquipment* equipment);
    bool InPolygon(uPoint &p, vector<uPoint> &poly);
    void ScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg, 
        const string& inversion, int equip_name, rclcpp::Time time);
    bool Isenable(string status, ScenesJudge::cEquip& equip);
    std::vector<agv_msgs::msg::AvoidObsta> ScanCheck(vector<ScenesJudge::cEquip>& laser_equip);
    vector<uPoint> SplitRegion(const int& equip_name, const int& area_name, const string& region_name);
    bool TimeoutCheck(const builtin_interfaces::msg::Time& time);
    bool LaserNumCheck(int& equip_name, const string& region_name, bool& flag);
    void PathCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr path_msg);
    void OdomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_msg);
    bool CheckVelState(const bool& cur_state, const bool& last_state);
};

} // namespace AgvObstaAvoid

#endif // LASER_OBSTA_AVOID_HPP